#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <iostream>
#include <string>
#include <sstream>
#include <std_msgs/Int16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Empty.h>
#include <stdlib.h>
#include <cstdio>
#include <cmath>

const int CMD_OFFSET = 1520;
const double C2PI = (2 * 3.1415926) / 16384;
const double PI = 3.1415926;

// 函数功能：将数据经由串口发送出去
// 入口参数1：[serial::Serial &ser]：     串口类名称
// 入口参数2：[std::string &serial_msg]:  要通过串口发送出去的字符串
int serial_write(serial::Serial &ser, std::string &serial_msg)
{
    ser.write(serial_msg);
    return 0;
}

// 函数功能：将从串口接收到的数据保存到数组中
// 入口参数1：[serial::Serial &ser]：     串口类名称
// 入口参数2：[std::string &serial_msg]:  从串口读取的字符串
int serial_read(serial::Serial &ser, std::string &serial_msg)
{
    serial_msg = ser.read(ser.available());
    return 0;
}

// CRC8
uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
{
    uint8_t crc_u = crc ^ crc_seed;

    for (int i = 0; i < 8; ++i)
    {
        crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
    }

    return crc_u;
}

// CRC8校验
uint8_t crc8(const uint8_t *buf, uint8_t len)
{
    uint8_t crc = 0;

    for (int i = 0; i < len; ++i)
    {
        crc = updateCrc8(buf[i], crc);
    }

    return crc;
}

// 字符转成16进制，a1为数组具体的一位。
std::string Char2Hex(char a1)
{
    std::string str = "";
    int n = a1; // n为a1的ASCII码
    for (int i = 2 * sizeof(char) - 1; i >= 0; i--)
    {
        str += "0123456789ABCDEF"[((n >> i * 4) & 0xF)];
    }
    return str;
}

int main(int argc, char **argv)
{

    setlocale(LC_CTYPE, "zh_CN.utf8"); // 设置编码，防止中文乱码

    // 初始化，节点名为serial_publisher
    ros::init(argc, argv, "serial_publisher");
    // 创建句柄seuNB，用于管理资源
    ros::NodeHandle nh;

    // 发布编码器位置话题
    std_msgs::Float64 encoder_position;
    ros::Publisher encoder_position_pub =
        nh.advertise<std_msgs::Float64>("/myserial_pub/encoder_position", 10);
    // 发布油门话题
    std_msgs::Int32 throttle;
    ros::Publisher throttle_pub =
        nh.advertise<std_msgs::Int32>("/myserial_pub/throttle", 10);
    // 发布正弦相位话题
    std_msgs::Float64 sine_phase;
    ros::Publisher sine_phase_pub =
        nh.advertise<std_msgs::Float64>("/myserial_pub/sine_phase", 10);
    // 发布正弦幅值话题
    std_msgs::Float64 sine_amplitude;
    ros::Publisher sine_amplitude_pub =
        nh.advertise<std_msgs::Float64>("/myserial_pub/sine_amplitude", 10);
    // 发布电压话题
    std_msgs::Float64 voltage;
    ros::Publisher voltage_pub =
        nh.advertise<std_msgs::Float64>("/myserial_pub/voltage", 10);
    // 发布电流话题
    std_msgs::Float64 current;
    ros::Publisher current_pub =
        nh.advertise<std_msgs::Float64>("/myserial_pub/current", 10);
    // 发布功耗话题
    std_msgs::Float64 power_consumption;
    ros::Publisher power_consumption_pub =
        nh.advertise<std_msgs::Float64>("/myserial_pub/power_consumption", 10);
    // 发布转速话题
    std_msgs::Float64 rotational_speed;
    ros::Publisher rotational_speed_pub =
        nh.advertise<std_msgs::Float64>("/myserial_pub/rotational_speed", 10);
    // 发布温度话题
    std_msgs::Int32 temperature;
    ros::Publisher temperature_pub =
        nh.advertise<std_msgs::Int32>("/myserial_pub/temperature", 10);

    // 实例化一个serial类
    serial::Serial ser;

    // 初始化串口相关设置
    ser.setPort("/dev/ttyUSB0");                               // 设置打开的串口名称:这里打开一个虚拟串口
    ser.setBaudrate(115200);                                   // 设置串口的波特率
    serial::Timeout to = serial::Timeout::simpleTimeout(1000); // 创建timeout
    ser.setTimeout(to);                                        // 设置串口的timeout

    // 打开串口
    try
    {
        ser.open(); // 打开串口
    }

    catch (const std::exception &e)
    {
        ROS_ERROR_STREAM("Unable to open port."); // 打开串口失败，打印日志信息，然后结束程序
        return -1;
    }

    // 判断串口是否成功打开
    if (ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port is opened.\n"); // 成功打开串口，打印日志信息
    }
    else
    {
        return -1; // 打开串口失败，打印日志信息，然后结束程序
    }

    ros::Rate loop_rate(50); // 指定循环频率50
    while (ros::ok())
    {
        // 将数据传入到serial_msg1中
        // 获取缓冲区内的字节数
        size_t n = ser.available();
        uint8_t buffer[1024];
        if (n != 0)
        {
            uint8_t buffer[1024];
            n = ser.read(buffer, n);
            unsigned int crcCode;
            unsigned int number1[18] = {
                buffer[0], buffer[1],   // 编码器位置
                buffer[2], buffer[3],   // 油门指令
                buffer[4], buffer[5],   // 正弦相位
                buffer[6], buffer[7],   // 正弦幅度
                buffer[8], buffer[9],   // 电压
                buffer[10], buffer[11], // 电流
                buffer[12], buffer[13], // 功耗
                buffer[14], buffer[15], // 转速
                buffer[16]              // 温度

            };
            unsigned int number2[2] = {buffer[17]}; // crc8校验
            // int i = sizeof(number1);
            // ROS_INFO_STREAM("i:"<<i);
            unsigned int *ptr = NULL;
            ptr = number1;
            crcCode = crc8(buffer, 17);
            number1[18] = crcCode;

            int value1 = crcCode;

            int value2 = number2[0];

            // 打印校验值
            //  ROS_INFO_STREAM("crc1"<<value1);
            //  ROS_INFO_STREAM("crc2"<<value2);

            int encoder_orginal = buffer[1] << 8 | buffer[0];             // 编码器位置
            int throttle_orginal = buffer[3] << 8 | buffer[2];         // 油门指令 （原始数*2-2100）得到小于200就等于零
            int x_orginal = buffer[5] << 8 | buffer[4];                   // x  （ ）
            int y_orginal = buffer[7] << 8 | buffer[6];                   // y   ( )
            int voltage_orginal = buffer[9] << 8 | buffer[8];             // 电压
            int current_orginal = buffer[11] << 8 | buffer[10];           // 电流
            int power_consumption_orginal = buffer[13] << 8 | buffer[12]; // 功耗
            int rotational_speed_orginal = buffer[15] << 8 | buffer[14];  // 转速
            int temperature_orginal = buffer[16];                         // 温度

            // ROS_INFO_STREAM("编码器位置：" << encoder_orginal);
            // ROS_INFO_STREAM("油门指令：" << a23);
            // ROS_INFO_STREAM("正弦相位：" << x_orginal);
            // ROS_INFO_STREAM("正弦幅度：" << y_orginal);
            // ROS_INFO_STREAM("电压：" << voltage_orginal);
            // ROS_INFO_STREAM("电流：" << current_orginal);
            // ROS_INFO_STREAM("功耗：" << power_consumption_orginal);
            // ROS_INFO_STREAM("转速：" << rotational_speed_orginal);
            // ROS_INFO_STREAM("温度：" << temperature_orginal);

            if (value1 == value2)
            {
                ROS_INFO("开始发布");
                // 发布编码器位置数据。编码器范围0-16383对应（0-360度）
                double encoder = encoder_orginal * 360 / 16384;
                encoder_position.data = encoder;
                encoder_position_pub.publish(encoder_position);
                ROS_INFO_STREAM("编码器位置：" << encoder_position.data);

                // 发布油门数据（原始数*2-2100）得到小于200就等于零
                if (throttle_orginal * 2 - 2100 < 200 || throttle_orginal * 2 - 2100 == 200)
                {
                    throttle.data = 0;
                }
                else
                {
                    throttle.data = throttle_orginal * 2 - 2100;
                }
                throttle_pub.publish(throttle);
                ROS_INFO_STREAM("油门：" << throttle.data);

                // 计算相位
                int x = x_orginal - CMD_OFFSET;
                int y = y_orginal - CMD_OFFSET;

                double phase = atan2(y, x) * 360 / (2 * PI);
                // 计算幅值
                double amplitude = sqrt(x * x + y * y) / 1000;
                if (amplitude < 0.05)
                {
                    amplitude = 0.0;
                }
                else
                {
                    amplitude = sqrt(x * x + y * y) / 1000;
                }

                // 发布正弦相位数据
                sine_phase.data = phase;
                sine_phase_pub.publish(sine_phase);
                ROS_INFO_STREAM("正弦相位：" << sine_phase.data);
                // 发布正弦幅值数据
                sine_amplitude.data = amplitude;
                sine_amplitude_pub.publish(sine_amplitude);
                ROS_INFO_STREAM("正弦幅值：" << sine_amplitude.data);
                // 发布电压数据
                voltage.data = voltage_orginal / 100;
                voltage_pub.publish(voltage);
                ROS_INFO_STREAM("电压：" << voltage.data << "V");
                // 发布电流数据
                current.data = current_orginal / 10;
                current_pub.publish(current);
                ROS_INFO_STREAM("电流：" << current.data << "A");
                // 发布功耗数据
                power_consumption.data = power_consumption_orginal / 10000;
                power_consumption_pub.publish(power_consumption);
                ROS_INFO_STREAM("功耗：" << power_consumption.data << "Ah");
                // 发布转数数据
                rotational_speed.data = rotational_speed_orginal / 100;
                rotational_speed_pub.publish(rotational_speed);
                ROS_INFO_STREAM("转数：" << rotational_speed.data);
                // 发布温度数据
                temperature.data = temperature_orginal;
                temperature_pub.publish(temperature);
                ROS_INFO_STREAM("温度：" << temperature.data);
            }
            else
            {
                ROS_INFO("校验码出错");
                return -1; // 打开串口失败，打印日志信息，然后结束程序
            }

            loop_rate.sleep();
        }
    }
    // 关闭串口
    ser.close();
    return 0;
}